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SENSOR  NOISE  AND  KALMAN  FILTER  FOR  AIDED 


INERTIAL  NAVIGATION  SYSTEM 
by 

Gurmohan  S.  Grewal 
ABSTRACT 


Inertial  Navigation  System,  barometric  altimeter,  TACAN,  and  ILS 
are  used  to  achieve  a  synergistic  combination  of  the  outputs  of  indi¬ 
vidual  subsystems.  Kalman  filter  is  used  to  provide  an  ideal  method 
for  data  processing  in  this  multisensor  navigation  system.  The  filter 
design  begins  with  the  development  of  mathematical  and  statistical 
error  models  to  describe  the  truth  system.  The  truth  model  is  sim¬ 
plified  and  reduced,  in  steps,  to  lower  the  computation  burden  on  the 
on-board  computer.  The  covariance  analysis  and  the  Monte  Carlo  methods 
of  testing  the  performance  of  the  Kalman  filters  based  on  reduced  and 
simplified  system  models  are  discussed.  Suggestions  for  further 
research  in  the  area  of  fault  detection  and  isolation  are  offered. 
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I.  INTRODUCTION 


Inertial  Navigation  Systems  (INS)  and  non-inertial  navigation  aids 
such  as  TACAN,  ILS,  Loran,  OMEGA,  navigation  satellites,  etc.,  have  been 
used  in  a  number  of  multisensor-based  navigation  systems.  Outputs  of  the 
Individual  subsystems  are  combined  synergistically.  The  software  accom¬ 
plishes  this  combination  Ideally  utilizing  the  data  from  the  subsystems  to 
yield  much  more  accurate  results  than  these  subsystems  can  provide  unaided. 
Data  processing  algorithm,  called  Kalman  filter,  provides  a  systemic  and 
logical  method  of  weighing  various  sources  of  information  to  produce  a  best 
estimate  of  the  quantities  of  interest. 

This  research  is  concerned  with  the  development  of  a  Kalman  filter  that 
combines  the  data  from  a  baro-inertial  navigation  with  the  range  and  bearing 
measurements  of  a  TACAN  system  during  the  cruise  portion  of  the  flight  and 
the  measurements  from  an  ILS  system  during  the  descent  and  the  final  approach 
phase  of  a  flight  (Ref  1) .  The  resulting  filter  provides  the  position  and 
the  attitude  of  the  aircraft. 

The  performance  of  a  Kalman  filter  is  dependent  upon  adequate  mathemat¬ 
ical  and  statistical  models  to  describe  the  true  system  including  system  and 
measurement  dynamics,  system  disturbances  and  measurement  errors,  and  Initial 
condition  information.  These  models  are  formulated  In  the  state  space. 

There  are  two  approaches  available  for  the  state  space  formulation  of  the 
models:  the  "total"  state  space  models  and  the  "error"  state  space  models. 

In  the  total  state  space  formulation,  position,  velocity,  and  attitude  are 
among  the  state  variables,  and  the  measurements  Include  accelerometer  out¬ 
puts  and  signals  from  TACAN  or  ILS.  The  resulting  vehicle  dynamics  equations 
are  nonlinear,  high  frequency,  and  are  not  adequately  developed  for  use  in 
the  Kalman  filter.  In  the  error  state  space  formulation,  the  errors  in  the 
Inertial  navigation  system  position,  velocity,  and  attitude  values  are  among 
the  state  variables,  and  measurements  are  composed  of  the  differences  between 
the  inertial  and  the  external  source  data.  The  resultant  vehicle  dynamics 
equations  for  the  error  state  space  formulation  are  low  frequency,  linear, 
and  fairly  well  developed  for  use  in  the  Kalman  filter  (Ref  2,  3,  4). 
Consequently,  the  error  state  space  formulation,  which  is  also  called 
"indirect"  filter,  is  adopted.  Further,  the  indirect  filter  can  be  imple¬ 
mented  in  two  ways:  feedforward  and  feedback.  In  the  case  of  feedforward. 


the  output  of  the  filter,  which  Is  the  optimal  estimates  of  the  errors 
between  the  Inertial  system  outputs  and  the  true  values.  Is  subtracted  from 
the  Inertial  system  outputs  to  obtain  the  best  available  estimate  of  the 
vehicle  position,  velocity,  and  attitude.  The  Inertial  system  Is  unaware  of 
the  existence  of  the  Kalman  filter.  The  Inertial  system  Is  free  to  drift 
with  unbounded  errors.  As  these  errors  get  large,  the  adopted  model  of  the 
Inertial  system  becomes  Invalid,  resulting  In  filter  "divergence."  On  the 
other  hand.  In  the  feedback  configuration,  the  output  of  the  filter,  which 
Is  the  error  between  the  true  values  and  the  Inertial  values.  Is  fed  back  to 
the  inertial  system  to  obtain  a  set  of  corrected  Inertial  outputs.  Thus  the 
Inertial  system  errors  are  not  allowed  to  grow  unbounded.  Moreover,  the 
filter  need  not  propagate  the  estimates  of  the  error  state  variables.  Hence, 
the  feedback  configuration  is  preferred. 

If  the  comprehensive  truth  models  are  used  in  the  development  of  Kalman 
filter,  the  resulting  filter  will  require  extensive  memory  and  computation 
time,  making  it  Impractical  for  the  limited  on-board  computer  to  handle  the 
problem.  The  computation  load  is  approximately  proportional  to  the  third 
power  of  the  number  of-  states  required  for  modelling  the  system  dynamics. 
Therefore,  simplified  models,  rather  than  the  truth  models,  are  used  in  the 
filter  development.  The  models  simplification  will  result  in  performance 
degradation.  In  order  to  make  intelligent  approximations  and  assumptions 
necessary  to  obtain  workable  models,  it  is  important  to  thoroughly  understand 
the  laws  governing  the  involved  system.  Resulting  performance  degradation 
can  be  analyzed  using  covariance  analysis  and  the  Monte  Carlo  methods.  The 
Air  Force  has  fully  developed,  unclassified,  transportable  software  packages 
for  vehicle  trajectory  generation  (Ref  5),  covariance  analysis  (Ref  6), 

Monte  Carlo  analysis  (Ref  7) ,  and  for  plotting  the  results  of  Monte  Carlo 
analysis  (Ref  8),  specifically  to  aid  the  testing  and  evaluation  of  the 
Kalman  filter. 

H.  OBJECTIVES 

The  effort  involved  for  this  Summer  Faculty  Program  had  two  main  objectives: 
(1)  To  provide  an  option  for  the  injection  of  random  errors  into  the  out¬ 
puts  of  the  INS,  TACAN,  and  air  data  sensor  models  of  the  Digital 
Avionics  Information  System  (DAIS).  Each  of  these  random  error  sources 
is  to  be  capable  of  interruption  by  the  setting  of  an  appropriate  flag 
bit  In  a  control  word. 


(2)  After  a  detailed  study  of  the  available  comprehensive  TACAN/lLS- 
alded  baro- inertial  navigation  system  models  (Ref  2,  3,  4,  9), 
develop  reduced  states  simplified  models  in  order  to  obtain  a  work¬ 
able  Kalman  filter.  The  simplifications  and  the  reductions  are  to 
be  implemented  in  steps  in  order  that  the  Kalman  filter  resulting 
from  each  approximation  can  be  tested  for  the  performance  sensitiv¬ 
ity  to  that  particular  approximation.  There  will  not  be  enough 
time  to  complete  the  performance  analysis,  but  the  strategy  for  the 
step-by-step  models  simplification  is  to  be  well  established  by  the 
end  of  this  ten-week  summer  period. 

III.  RANDOM  ERRORS  INJECTION  INTO  DAIS  SENSOR  MODELS 

In  order  to  simulate  various  error  conditions,  a  control  word  is 
defined  with  the  following  bit  assignments: 


The  DEC-10  FORTRAN  statement  for  the  control  word  Is  Inserted  in  the 
SCEN  subroutine  of  DAIS: 

IERROR  »  "OXXXXX" 

-  INS  Latitude  Error 

-  INS  Longitude  Error 

-  TACAN  Range  Error 

-  TACAN  Bearing  Error 

-  Air  Data  Error 

Default  value  IERROR  =  "011111" 

1  ,  NO  Noise 

Octal  Digit  Values  =  2,1  Noise 

4,5  Noise 

The  control  word  is  set  by  the  operator  at  the  beginning  of  the  simulation 
run. 

3.1  Random  Noise  Model 

A  normally  distributed  random  variable  N  of  zero  mean  and  unit 
variance  is  obtained  from  two  Independent  samples  and  U2  of  uniform 
distribution  between  zero  and  one  by  the  following  equation: 

A  second  normally  distributed  is  obtained  from  the  above  equation  by 
changing  the  SIN  to  COS: 

On  an 

The  time  correlation  is  introduced  as  follows: 

x*.  -  *~f>  XA_,  ijO-  *  Wfe)  A4, 

where 

n  =  nth  iteration 
6  t  =  iteration  Interval 
^  =  correlation  time 


The  error  eQ  of  mean  M  and  standard  deviation  is  obtained  by  the  fol¬ 
lowing  equation: 

~  O'**  -+  M 

Since  this  random  noise  is  used  repeatedly,  a  subroutine  called 
RNOISE  is  generated  for  it.  FORTRAN  coding  for  this  subroutine  is  as 
follows: 


SUBROUTINE  RNOISE(XLAST ,DELT , TAU , SIGMA,  ERR , XGUR) 

DOUBLE  PRECISION  X,X1,X2,RH0,ERR,XCUR,XLAST,PI2 

IF (X2 .EQ . 0 . )  GO  TO  5 

X=X2 

X2=0. 

GO  TO  10 
5  CALL  RDN(Ul) 

CALL  RDN(U2) 

PI2=6.28318530717958648D0 

X1=DSQRT (-2 .DO*ALOG(Ul) )*DCOS(PI2*U2) 

X2=DSQRT(— 2 -DO*ALOG(U1) ) *DSIN(PI2*U2) 

X=X1 

10  RHO=DEXP ( -DELT/TAU) 

XCUR=RH0*XLAST+DSQRT(1-RH0**2) *X 
ERR=SIGMA*XCUR 
RETURN 
,  END 


The  variable  X2  needs  to  be  zeroed  at  the  beginning  of  the  simulation. 
3.2  INS  Error  Model 

The  INS  error  model  Introduces  errors  into  the  horizontal  naviga¬ 
tion  channels.  These  errors  have  the  following  parameters: 

Velocity  Error  Standard  Deviation  *=  0.707  n.m./hr.  per  axis 
Velocity  Error  Mean  Error  =*  0  per  axis 

Velocity  Error  Correlation  Time  *  30  min.  per  axis 


Since  the  INS  model  does  not  Integrate  velocity,  the  velocity  error  is 
Integrated  to  form  position  errors  to  be  added  to  INS  position  outputs. 
First,  two  velocity  errors  and  Vg  are  derived.  These  velocity 

errors  are  integrated  to  obtain  the  latitude  error  and  the  longitude 

error  ,  r 

—  r> -t- 

where  Rc  is  the  radius  of  the  earth.  Then  the  noise-corrupted  direction 
cosines,  longitude,  and  velocity  outputs  are: 

C  d.  C>(  ^ 

-  c.*^-  -f  Cic&'hd'y 

$  cb  -t*  ^ 

V*/  '  V,u  -fr  ^  ^ 

VfiT  -  \/j5~  *t  £ 

where  is  the  wander  angle.  The  following  codes  are  inserted  at  the 
end  of  INSIN  and  INSNAV,  respectively: 


Code  To  Be  Inserted  In  INUIN  Subroutine 

DOUBLE  PRECISION  XLINVE,XLINVN,DLINVE,DLINVN,PI2 
PI2=6 . 2831853O717958648D0 
CALL  RDN(Ul) 

CALL  RDN(U2) 

XLINVE=DSQRT (-2 .DO*ALOG(U1) )*COS (PI2*U2) 

* 

XLINVN=DSQRT  (-  2  .DO*ALOG  (Ul)  )  *SIN  (PI  2*U2) 

DELLAT=0 . 

DELLNG=0 . 

SDINVE=(SQRT(2.)/2.)/(3600.*FPSKTS) 

TAUINV=1800 . 

SDINVN=SDINVE 

IF((IERROR. AND. "000007) .EQ. "000001)  SDINVN=0. 

IF ( ( IERROR .AND . "000007 ) .EQ. "000004)  SDINVN=5*SDINVN 
IF ((IERROR. AND. "000070) .EQ. "000010)  SDINVE=0 
IF ((IERROR. AND. "0000 70) .EQ. ”000040)  SDINVE=5*SDINVE 


Code  To  Be  Inserted  In  INUNAV  Subroutine 


DOUBLE  PRECISION  XCINVE.XCINVN 

IF((IERROR. AND. "000077) .EQ. "000011)  GO  TO  100 

CALL  RNOISE(XLINVE,DTINUS,TAUINV,SDINVE,DLINVN,XCINVN) 

CALL  RNOISE (XLINVN .DTINUS ,TAUINV , SDINVN.DLINVE .XCINVE) 

XLINVE-XCINVE 

XLINVN-XCINVN 

VELEFS*VELEFS+DLINVE 

VELNFS=*VELNFS+DLINVN 

DELLAT=DELLAT+(DLINVN / EARADF ) *DTINUS 

DELLNG=DELLNG+(DLINVE/ (EARADF*COS (ANLATR) ) ) *DTINUS 

FLNGOR=FLNGOR+DELLNG 

CXXDIR=CXXDIR-COS (WANDER) *S IN( ANLATR) *DELLAT 
CXYDIR=CXYDIR+S IN (WANDER) *SIN (ANLATR) *DELLAT 
CXZDIR=CXZDIR+COS (ANLATR) *DELLAT 
100  CONTINUE 

3.3  TACAN  Error  Model 

Random,  time-correlated  errors,  having  the  following  characteristics, 
are  added  to  the  TACAN  range  and  bearing  measurements: 

Range  Bias  Error  Standard  Deviation  =  2000  ft. 

Bearing  Bias  Error  Standard  Deviation  =  2  deg. 

Range  and  Bearing  Correlation  Time  8  5  sec. 

Range  and  Bearing  Mean  Error  =  0 


The  following  code  is  inserted  into  the  TACAN  and  CHNTAC  subroutines  of 
DAIS. 


Code  To  Be  Inserted  Into  TACAN  Subroutine 

DOUBLE  PRECISION  XLTACR,XLTACB,PI2 
CALL  RDN(Ul) 

CALL  RDN(U2) 

XLT ACR=DSQRT ( - 2 .D0*ALOG(Ul) )*DC0S (PI2*U2) 
XLTACB=DSQRT(-2.D0*ALOG(Ul))*DSIN(PI2*U2) 


SDTACR=2000 . 

SDTACB=2 . /DEGPRR 
TAUTACrS . 

IF( (IERROR . AND . "000700) . EQ . "000100) 
1F( (IERROR. AND ."000700) .EQ ."000400) 
IF ( (IERROR .AND . "007000) . EQ . ”001000) 
IF((IERROR.AND. "007000) .EQ . "004000) 
DTTACS=DELTIM* (8 .  / ITRATE (MODELN) ) 


SDTACR=0 . 
SDTACR=5.*SDTACR 
SDTACB=0. 
SDTACB=5 . *  SDTACB 


Code  To  Be  Inserted  Into  CHNTAC  Subroutine 

DOUBLE  PRECISION  XCTACR.XCTACB 
IF( (IERROR.AND. "007700) .EQ. "001100)  GO  TO  100 
CALL  RNOISE  (XLTACR.DTTACS .TAUTAC ,SDTACR,DLTACR,XCTACR) 
CALL  RNOISE  (XLTACB , DTTACS , TAUTAC , SDTACB ,DLTACB ,XCTACB) 
XLTACR=XCTACR 
XLTACB=XCTACB 
CRANGF=CRANGF+DLTACR 
CAZMTR=CAZMTR+DLTACB 
100  CONTINUE 


3.4  Air  Data  Error  Model 

In  order  to  corrupt  true  air  speed  (TAS)  without  disturbing  the 
calculation  of  altitude,  the  temperature  output  from  the  air  data  sensor 
model  is  perturbed.  The  sensitivity  of  the  TAS  to  changes  in  the 
temperature  Is  related  as  follows: 


dTR  q  d_  (-tas) 


where  TR  is  the  temperature  in  deg.R. 
k%,  1  ,  is: 


A  percentage  error  in  the  TAS  of 

i  -+z.<  //oo) 


where  R  is  a  random,  time-correlated  variable  with  the  following  parameters 


Percentage  Error  in  TAS  =  3%,  1 
Correlation  Time  =  3  min. 


The  following  code  Is  Inserted  in  the  ADC IN  and  ADC  subroutines  to 
achieve  the  required  temperature  corruption. 


To  Be  Inserted  In  ADCIN  Subroutine 

DOUBLE  PRECISION  XLADCT 
CALL  RDH(Ul) 

CALL  RDN(U2) 

XLADCT=DSQRT(-2.DO*ALOG(U1))*COS (PI2*U2) 

SDADCT=3 . 

TAUADC=180 . 

IF((IERROR.AND. "070000) .EQ. "010000)  SDADCT=0. 

IF ((IERROR. AND. "070000)  .EQ. "040000)  SDADCT=*5 .*SDADCT 
DTADCS=DELTIM* (64 . /ITRATE (MODELN) ) 


To  Be  Inserted  Into  ADC  Subroutine 
DOUBLE  PRECISION  XCADCT 

IF( (IERROR. AND. "070000) .EQ. "010000)  GO  TO  100 

CALL  RNOISE (XLADCT .DTADCT , TAUADC , SDADCT .DLADCT ,XCADCT) 

XLADCT=XCADCT 

TMALTR=TMALTR*(l.+2 .*DLADCT/100.) 

100  CONTINUE 


IV.  THE  FUNDAMENTAL  STRUCTURE  OF  KALMAN  FILTER 
The  system  state  Is  described  by: 


r  y  t-  V-  IH  O  kb*  Wl.  AO  L/J  • 

(to*  0  -  t  (*+'  /  *)  *0)  -t  ftCK)  Q-M  J 


and  the  measurements  are  described  by 

^00=  HU)  fcU)  -t^U) 

where. 


X(k) 

U(k) 

W(k) 

Z(k) 


n-by-1 

r-by-1 

s-by-1 

m-by-1 


state  vector  at  time  tk 
deterministic  input  vector 
driving  noise  vector 
measurement  vector 


V(k)  *  m-by-1 
I  (k+l,k)  =  n-by-n 
l[(k)  =  n-by-r 
G(k)  =  n-by-s 
H(k)  =  m-by-n 


measurement  noise  vector 
state  transition  matrix 
deterministic  input  matrix 
noise  input  matrix 
measurement  matrix 


and,  W(k)  and  V(k)  are  zero  mean  white  noise  sequences  with  known 


covariance : 


J- 

Ul  1  o  J4 

-o 


J-fc 

J  -a* 
J 


The  Kalman  filter  updates  the  state  estimate  X(k)  and  error  covariance 
P(k)  at  the  measurement  time  by: 

■=.  &  (.*")  -t'dC*)  ^(K) 

PM  •=  P CvO -  * Co  WCOPOc) 

From  one  measurement  to  the  next,  the  state  and  error  covariance  are 
extrapolated  as  follows: 

p  (Kf)  =.  J(V;k-i)  P(f ->) §(«, 4(k~ 0 $ 

where  K(k)  is  the  Kalman  filter  gain  and  Y(k)  is  the  innovation  process, 

KW  •=•  PCk-)w(k37!/  (kf 1 

yCft*) 


and 


l(Jk) 

=•  wc *)£(*') 

The  above  recursive  relations  are  Initiated  by  apriori  knowledge  of 
the  state  and  the  error  covariance  at  time  .  Thus, 

A  A 

£to)  •*-  25 d 

P,Co)  ~  P* 

V.  FULL-SCALE  STATE  SPACE  ERROR  MODELS 

The  comprehensive  error  models,  which  are  the  departing  point  of 
the  models  simplifications,  are  presented  in  this  section  (Ref  1). 


5.1  Inertial  System  Error  Model 

With  the  latitude-longitude  mechanization  (x:  level  east,  y:  level 
north,  z:  up)  of  the  platform  orientation,  the  full-scale  inertial 
system  errors  model  Is  developed  In  terms  of  the  following  state  variables 

(1)  Position  errors^S^,  defined  as  the  computed  values 

minus  the  true  values, 

(2)  Velocity  errors  defined  similarly, 

(3)  Misalignment  angles  defined  as  angles  from  the  computer 

axis  to  the  platform  axes, 

(4)  Accelerometer  errors  i  Oi 

(5)  Platform  drift  errors  ;  £  f  £u) 

Because  of  the  limited  cross-feed  between  the  vertical  loop  and  the 
level  loop  and  due  to  the  fact  that  the  vertical  loop  needs  to  be 
stabilized  by  the  barometer  altimeter  measurement  of  the  altitude,  the 
vertical  and  the  level  loops  are  decoupled. 
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5.1.1  Inertial  System  Level  Loop  State  Equations 
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where  O/^o)^ 

tt/ 

R  =  radius  of  the  earth 

L  =  latitude 

Wg  =  Schuler  frequency 

Wfe  =  earth  rotation  rate 

a^  =  accelerometer  outputs,  I=e,  n,  u 

WjL  =  gyro  outputs,  i=e,  n,  u 

T  =  correlation  times 

='  white  zero-mean  Gaussian  noises 
«■  errors  in  the  gravity  vector,  i=e,  n 


5.1.2  Baro-Inertial  Vertical  Loop  State  Equations 
The  vertical  state  equations  are: 
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where 


Ta-J  •*.  £ 

^  ^  %•  ^  \y 

The  Ct  values  are  selected  to  obtain  a  desired  vertical  loop  time  constant. 

5.2.1  Measurement  Error  Model  For  TA.CAN  (Ref  _1_I 

The  bearing  and  the  range  error  measurements  are: 
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The  TACAN  measurement  biases  b  and  b  are  represented  as  state  variables, 
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5.2.2  Measurement  Error  Model  For  ILS  (Ref  1) 

The  localizer  measurement  and  the  glldescope  measurement  S  are 
modelled  as  follows: 


s=  sw*S's-vs 


where  V<^  are  zero  mean  white  noise,  and  O  5  are  zero  mean 

exponentially  correlated  variables  which  will  produce  two  extra  state 
variables.  The  measurement  errors  to  be  used  are: 
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5.3  Simplified  Models  (Ref  1) 

A  Kalman  filter  based  on  the  comprehensive  models  of  the  previous 
section  is  Impractical  because  of  the  excessive  computation  burden  on 
the  on-board  computer.  In  order  to  develop  a  workable  Kalman  filter, 
the  underlying  models  must  be  simplified.  These  simplifications  will 
result  in  the  filter  performance  degradation.  Some  of  these  simplifi¬ 
cations  may  result  in  an  unacceptable  loss  in  the  performance  of  the 
resultant  filter.  Thus,  it  is  essential  that  the  performance  sensitivity 
to  each  simplification  be  evaluated.  The  rest  of  this  section  outlines 
some  of  the  feasible  simplifications. 


Hie  step-by-step  simplifications  are  performed  in  the  following 

order: 

Step  1  All  the  terms  in  the  comprehensive  state  equations  which  are 
of  the  order  of  magnitude  of  w^e  are  ignored,  resulting  in 
removal  of  the  weak  coupling  between  the  states.  The  result 
will  not  be  the  reduction  of  the  number  of  states,  but  will 
make  the  state  transition  matrix  sparse. 

Step  2  Model  the  accelerometer  output  uncertainties  by  a  white  noise 
of  appropriate  power.  This  will  result  in  reducing  the  number 
of  states  by  two. 

Step  3  The  three  states  modelling  the  gyro  drift  rates  are  removed  by 

replacing  the  gyro  drift  by  a  white  noise  of  appropriate  intensity. 

After  the  above  three-step  simplification,  the  state  equations 


become: 
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5.3.2  Simplified  Vertical  Loop 

Although  the  undamped  vertical  loop  is  unstable  for  the  short 

period  during  approach  and  landing,  the  Kalman  filter  can  be  developed 


using  the  undamped  vertical  loop  dynamics. 

where  is  a  white  noise  representing  the  term 


5.3.3  Simplified  TACAN  Model 

The  state  due  to  the  TACAN  range  bias  is  removed. 
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